算法实时性能优化总结

1. 什么是实时性

实时性指系统或程序在特定时间约束下对外部事件作出响应的能力，其核心是任务能够严格在固定时间内执行，一般这种任务都是周期性任务。不同任务对固定时间（时限）的要求不一样，例如导弹姿态调整需在50 μs内完成; 高精度数控机床的插补周期需在 0.1–1 ms 内完成; 机器人运动控制，时限一般是ms级; 服务机器人交互，延迟容忍度较高（100–500 ms），但需避免明显卡顿影响用户体验。

2. 机器人控制系统中哪些任务需要实时响应

• 运动控制(T1): 一般控制周期在0.5ms-8ms左右。在机器人运动过程中，每个周期内，控制系统需要保证有指令下发给关节驱动。

• 总线通信(T2): 配合运动控制，需要在对应的时间内及时下发控制命令和反馈机器人状态。

• 伺服跟踪(T3): 如遥操作跟踪响应一般在20ms以内; 视觉伺服响应一般在40ms左右; 动态调速一般响应在100ms以内。跟踪类任务的时限是机器人系统响应外部变化的性能指标。伺服跟踪底层由运动控制实现，属于上层的任务。

• 缩减模式(T4): 安全类任务，一般反应时间在300ms以内(包括信号检测时间和程序反映时间)，具体数值有待商榷。

• 防护停止、急停(T5): 安全类任务，一般反应时间在200ms以内(包括信号检测时间和程序反映时间)，具体数值有待商榷。

• 动态避障(T6): 安全类任务，一般反应时间在100ms以内， 具体数值取决与机器人的运动速度。

3. 算法库中耗时计算分类

以下时间都是基于研发工控机算力的预测，具体数值可以实测

参数辨识: 跟具体任务相关，如负载辨识、动力学辨识，耗散达到s级(一般不要求实时性)

路径规划、轨迹优化: 具体耗时可能达100ms级别

速度规划: 1-10ms级别

轨迹插值: 100us级别

力控: 1-2ms级别

逆解/动力学: 50us-1ms级别(具体与机器人的复杂度有关)

其他运动学接口: 20us以内

4. 算法针对实时任务的设计方案

4.1 运动控制

软件需要周期性从规划器中取出控制目标，然后调用控制器去实现(达到)这些目标。

• 软件在向规划器中插入运动指令时，会触发路径生成、速度规划和轨迹优化等耗时操作。目前软件层的解决方案是增加轨迹点缓冲区，随着轨迹优化算法的复杂度提升，目前的策略可能失效。

• 算法针对此问题的解决方案是在规划层定义实时接口和非实时接口，由多线程调用。其中像tpAddPositionLine、tpAddPoints等接口属于非实时接口，像tpTrajectoryTracking、tpUpdateCycle等接口属于实时接口。

• 规划层最终实现的效果是在插入运动指令时（除第一次），轨迹执行队列中始终有已经规划完成的轨迹段供软件调用(通过tpUpdateCycle)，因此算法只用保证tpUpdateCycle函数的实时性(目前只有轨迹插值和状态维护操作)。

  • 对于一般的轨迹段(运动时间大于200ms)， 所以软件插入运动指令后，轨迹规划的耗时需要小于这个时限
  • 轨迹执行队列中一般不需要存储多段轨迹(否则会影响任务 T3/T4 的响应性能)，只需要保证软件插入运动指令时，轨迹执行队列的运动时间能够对冲新插入运动指令的计算耗时
  • 轨迹执行队列中为什么需要有轨迹段？ 因为如果没有，每次插入运动指令后，都需要等待轨迹规划完成机械臂才能运动，会对运动效率有所影响。
  • 还需要介绍前瞻和动态交融(TODO)

• 控制器层的优化主要是引入RobotState类，将一些中间变量或者状态变量存储起来，避免重复计算。

  • 针对ARCS软件目前的用法，RobotState类后面需要开放给软件层，避免多个ARAL算法实例间的重复计算

在实时控制线程中，非必要，不要触发耗时操作，如计算逆解等。

4.2 缩减模式

算法提供了非实时接口:

ARAL_API_COMMON(1.0) int tpSetVelocityAndAccelerationLimits(const RLJntArray& joint_vel_limits, const RLJntArray& jint_acc_limits,const Array2d& cartesian_vel_limits, const Array2d& cartesian_acc_limits, const unsigned int buff_size) = 0;

• 这个函数会触发对轨迹执行队列和轨迹缓冲队列中的所有路径根据新的运动约束进行速度规划和轨迹优化。
• 针对这种耗时操作，算法会在原轨迹上采样一定的点放到算法的缓冲区，来抵消计算耗时(tpUpdateCycle函数总是会先取算法缓冲区里的轨迹点)
• 具体缓冲点的个数由buff_size来确定，后面可改成由算法内部确定(需要明确路径长度和计算量的对应关系，新开发的TOPP算法可以找到这种关系)
• 此接口的实际响应时间=buff_size*控制周期

4.3 动态调速

算法提供了非实时接口:

ARAL_API_COMMON(1.0) int tpScaleVelocityAndAcceleration(const double& vel_scal, const double& acc_scal, const unsigned int buff_size) = 0;

• 接口的用法跟缩减模式类似
• 软件层可以做一个延迟响应，比如延迟20ms，如果收到的vel_scal没有变化，再下发给算法。
• acc_scal参数一般不能由用户调整，算法内部可以做自适应，后面可以废弃

4.4 防护停止、急停

算法提供了非实时接口:

ARAL_API_COMMON(1.0) int tpStop(const StopType& type, const double& acc_ratio, const unsigned int buffSize) = 0;

• 这个函数只会触发速度规划，因此响应时间会在10ms以内，具体的buff_size = 10ms / 控制周期.
• 规划完成后，在使用tpUpdateCycle取点时，算法内部会根据机器人的状态不断重新进行速度规划(主要是考虑速度和加速度性能指标的变化)，这个操作的耗时在3-4ms左右，是一个性能瓶颈。 目前的应对策略是降低调整频率。

4.5 轨迹跟踪

算法提供了两个实时接口:

ARAL_API_COMMON(1.0) int tpTargetMotionTracking(const PathPoint& point, const MoveProperty& move_property, const unsigned int buffSize, const bool verbose = true) = 0;

ARAL_API_EXTENSION(1.0) double tpTrajectoryTracking(const PathPoint& point, const MoveProperty& move_property, const double& smooth_scale, const double& delay_sacle, const TrajectoryTrackingStatus& status) = 0;

• tpTargetMotionTracking跟踪的是最终的目标(具体可以是关节空间的位置或者迪卡尔空间的速度)，接口响应时间在5ms以内，跟踪延时由跟踪的运动参数确定。建议调用频率在20hz以内.
• tpTrajectoryTracking跟踪的是连续的轨迹点(只支持关节空间)，最后会完全贴合用户的路径，接口响应时间在3ms以内，跟踪延时由跟踪的运动参数确定。建议调用频率在100hz以内.

4.6 动态避障

• 对于AGV，一般是根据相机或者雷达反馈进行局部路径规划
• 对于机械臂，一般用于视觉伺服场景，结合动态路径规划和B样条拟合来实现，目前已开发完成，未测试性能

5. 提升实时性的计划

• 分离出robot_model和robot_state类指针，供用户传入到各个功能接口，避免状态的重复计算
• 简化优化问题的结构或者精简模型，调用高效的LP/QP/SDP算法进行优化
• 避免空间或者时序上的重复计算，采用中间状态变量或者近似变量
• 尽量让分支有规律性，使用likely()/unlikely()或编写无分支代码(实时线程的总体计算量做到稳定且可控)
• 合理规划相关状态的计算时机，不做无用计算(如逆解）

对计算敏感，在开发过程中总是思考怎么去优化代码片段、精简算法结构从而减少计算量是算法开发工程师的基本素质！